#include <iostream>
#include <fstream>
#include "PipesSetCloud.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <windows.h>


#include <pcl/visualization/pcl_visualizer.h>

//点云降采样参数
constexpr float SAMPLE_LEAF_SIZE1 = 0.001f;
constexpr float SAMPLE_LEAF_SIZE2 = 0.001f;

//点云范围
constexpr float MIN_X = 0.23f;
constexpr float MAX_X = 1.0f;
constexpr float MIN_Y = -0.28f;
constexpr float MAX_Y = 0.27f;
constexpr float MIN_Z = 0.15f;
constexpr float MAX_Z = 0.28f;

////统计离群值参数
//constexpr int K = 25;
//constexpr float THRESHOLD_SCALE = 0.005f;
constexpr int KSearch1 = 20;
constexpr float fRadiusSearch1 = 0.001f;
constexpr int KSearch2 = 20;
constexpr int mean_k2 = 20;
constexpr double std_dev_mul_thresh2 = 0.01;
constexpr float fRadiusSearch2 = 0.001f;

//聚类分割参数
constexpr double Tolerance = fRadiusSearch1 * 1.5;

static std::string GetExeDirectory() {
    char buffer[MAX_PATH];
    GetModuleFileName(NULL, buffer, MAX_PATH);
    std::string::size_type pos = std::string(buffer).find_last_of("\\/");
    return std::string(buffer).substr(0, pos);
}



int main(int argc, char** argv)
{
    std::ofstream outFile("OutputData.txt");
    SetConsoleOutputCP(CP_UTF8);
    //读取点云数据
    PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);

    // 构建完整的文件路径
    std::string exeDir = GetExeDirectory();

    std::cout << "Executable directory: " << exeDir << std::endl;
    std::string filename = "1.ply";
    //std::string plyFilePath = parentDirStr + "\\" + filename;
    std::string plyFilePath = exeDir + "\\..\\" + filename;

    // 读取点云数据
    if (pcl::io::loadPLYFile<pcl::PointXYZ>(plyFilePath, *cloud) == -1) // 替换 "your_pcd_file.pcd" 为您的点云文件路径
    {
        PCL_ERROR("Couldn't read Cloud file. \n");
        return (-1);
    }
    PipesSetCloud::Ptr pipesSetCloud(new PipesSetCloud(cloud));
    std::cout << "读取点云成功，原始点云共有：" << pipesSetCloud->getOriginalCloud().size() << "个点" << std::endl;

    pipesSetCloud->DisplayCloud(EM_CLOUD_TYPE_ORIGINAL);
    pipesSetCloud->PassThroughFilter(MIN_X, MAX_X, MIN_Y, MAX_Y, MIN_Z, MAX_Z);
    std::cout << "直通滤波后点云共有" << pipesSetCloud->getPassThroughFilteredCloud().size() << "个点, 共滤除了" << pipesSetCloud->getOriginalCloud().size() - pipesSetCloud->getPassThroughFilteredCloud().size() << "个点" << std::endl;
    pipesSetCloud->DisplayCloud(EM_CLOUD_TYPE_PASS_THROUGH_FILTERED);
    std::cout << "开始双线程处理" << std::endl;
    pipesSetCloud->DirFiler(KSearch1, fRadiusSearch1, KSearch2, mean_k2, std_dev_mul_thresh2, fRadiusSearch2);
    pipesSetCloud->DisplayCloud(EM_CLOUD_TYPE_DIR_FILLED);
    std::cout << "Start splitting the point cloud" << std::endl;
    pipesSetCloud->CloudSegmentation(Tolerance);

    if (pipesSetCloud->pEndFaceClouds.empty())
    {
        std::cout << "Ring not found" << std::endl;
        return 0;
    }

    //遍历pipesSetCloud->pEndFaceClouds分别进行圆环拟合并展示
    EndFaceCloud::Ptr pEndFaceCloud;
    int i = 0,j = 0;
    std ::vector<TorusEdgeCloud> edgeClouds;
    for (auto iter = pipesSetCloud->pEndFaceClouds.begin(); iter != pipesSetCloud->pEndFaceClouds.end(); iter++) {
        i++;
        pEndFaceCloud = std::make_shared<EndFaceCloud>(**iter);
        std::cout << "Start the ring fitting" << std::endl;
        //pEndFaceCloud->TorusFit();
        TorusEdgeCloud edgeCloud;
        pEndFaceCloud->EdgeExtraction(edgeCloud);
        if (edgeCloud.IsNormalShape()){
            std::cout << "The ring fit is complete" << std::endl;
        }
        else {
            std::cout << "Ring" << i << " failed to fit." << std::endl;
            continue;
        }

        //pEndFaceCloud->Display();
        if (edgeCloud.TorusFit() != 1) {
            std::cout << "Ring" << i << " failed to fit."<< std::endl;
            continue;
        }
        j++;
        outFile << "Center of the ring " << j << " is: " << edgeCloud.getCenterX() << " " << edgeCloud.getCenterY() << " " << edgeCloud.getCenterZ() << std::endl;
        outFile << "The axis of the ring " << j << " is: " << edgeCloud.getAngleA() << " " << edgeCloud.getAngleB() << " " << edgeCloud.getAngleC() << std::endl;
        edgeClouds.push_back(edgeCloud);
        edgeCloud.Display();
    }
    outFile.close();
    i = 0;
    return 0;
}